/********************************** (C) COPYRIGHT *******************************
 * File Name          : Peripheral.C
 * Author             : WCH
 * Version            : V1.0
 * Date               : 2018/12/10
 * Description        : Peripheral slave multi-connection application, initialize 
 *                      broadcast connection parameters, then broadcast, after connecting
 *                      to the host, request to update connection parameters,
 *                      and transmit data through custom services.
 *********************************************************************************
 * Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd.
 * Attention: This software (modified or not) and binary are used for 
 * microcontroller manufactured by Nanjing Qinheng Microelectronics.
 *******************************************************************************/

/*********************************************************************
 * INCLUDES
 */
#include "CONFIG.h"
#include "devinfoservice.h"
#include "gattprofile.h"
#include "peripheral.h"
#include "ota.h"
#include "OTAprofile.h"

#include "BleCheck.H"

/*********************************************************************
 * MACROS
 */

/*********************************************************************
 * CONSTANTS
 */

/*********************************************************************
 * CONSTANTS
 */

// How often to perform periodic event
#define SBP_PERIODIC_EVT_PERIOD              1600

// How often to perform read rssi event
#define SBP_READ_RSSI_EVT_PERIOD             3200

// Parameter update delay
#define SBP_PARAM_UPDATE_DELAY               1600

// PHY update delay
#define SBP_PHY_UPDATE_DELAY                 2400

// What is the advertising interval when device is discoverable (units of 625us, 80=50ms)
#define DEFAULT_ADVERTISING_INTERVAL         80

// Limited discoverable mode advertises for 30.72s, and then stops
// General discoverable mode advertises indefinitely
#define DEFAULT_DISCOVERABLE_MODE            GAP_ADTYPE_FLAGS_GENERAL

// Minimum connection interval (units of 1.25ms, 6=7.5ms)
#define DEFAULT_DESIRED_MIN_CONN_INTERVAL    50

// Maximum connection interval (units of 1.25ms, 100=125ms)
#define DEFAULT_DESIRED_MAX_CONN_INTERVAL    200

// Slave latency to use parameter update
#define DEFAULT_DESIRED_SLAVE_LATENCY        6

// Supervision timeout value (units of 10ms, 100=1s)
#define DEFAULT_DESIRED_CONN_TIMEOUT         100

// Company Identifier: WCH
#define WCH_COMPANY_ID                       0x07D7

/*********************************************************************
 * TYPEDEFS
 */

/*********************************************************************
 * GLOBAL VARIABLES
 */

/*********************************************************************
 * EXTERNAL VARIABLES
 */

/*********************************************************************
 * EXTERNAL FUNCTIONS
 */

/*********************************************************************
 * LOCAL VARIABLES
 */
uint8_t Peripheral_TaskID = INVALID_TASK_ID; // Task ID for internal task/event processing

// GAP - SCAN RSP data (max size = 31 bytes)
static uint8_t scanRspData[31] = {
        // connection interval range
        0x05,// length of this data
        GAP_ADTYPE_SLAVE_CONN_INTERVAL_RANGE,
        LO_UINT16(DEFAULT_DESIRED_MIN_CONN_INTERVAL), // 100ms
        HI_UINT16(DEFAULT_DESIRED_MIN_CONN_INTERVAL),
        LO_UINT16(DEFAULT_DESIRED_MAX_CONN_INTERVAL), // 1s
        HI_UINT16(DEFAULT_DESIRED_MAX_CONN_INTERVAL),

        // Tx power level
        0x02,// length of this data
        GAP_ADTYPE_POWER_LEVEL,
        0, // 0dBm
           // complete name
        0x0C, // length of this data
        GAP_ADTYPE_LOCAL_NAME_COMPLETE,
};

// GAP - Advertisement data (max size = 31 bytes, though this is
// best kept short to conserve power while advertisting)
static uint8_t advertData[] = {
        // Flags; this sets the device to use limited discoverable
        // mode (advertises for 30 seconds at a time) instead of general
        // discoverable mode (advertises indefinitely)
        0x02,// length of this data
        GAP_ADTYPE_FLAGS,
        DEFAULT_DISCOVERABLE_MODE|GAP_ADTYPE_FLAGS_BREDR_NOT_SUPPORTED,

        // service UUID, to notify central devices what services are included
        // in this peripheral
        0x03,// length of this data
        GAP_ADTYPE_16BIT_MORE, // some of the UUID's, but not all
        LO_UINT16(SIMPLEPROFILE_SERV_UUID),
        HI_UINT16(SIMPLEPROFILE_SERV_UUID),

        0x09,
        0xff,
        0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
};

// GAP GATT Attributes
uint8_t attDeviceName[GAP_DEVICE_NAME_LEN] = "Demo";

// Connection item list
static peripheralConnItem_t peripheralConnList;

uint8_t peripheralMTU = ATT_MTU_SIZE;

// OTA IAP VARIABLES
/* OTA communication frame */
OTA_IAP_CMD_t iap_rec_data;

/* OTA analysis results */
uint32_t OpParaDataLen = 0;
uint32_t OpAdd = 0;
uint16_t block_buf_len = 0;
uint32_t prom_addr = 0;

/* Flash data temporary storage */
__attribute__((aligned(8)))  uint8_t block_buf[512];

/* IMAGE jump function address definition */
typedef int (*pImageTaskFn)(void);
pImageTaskFn user_image_tasks;

/* Flash erase */
uint32_t EraseAdd = 0;      //Removal address
uint32_t EraseBlockNum = 0; //Number of blocks that need to be erased
uint32_t EraseBlockCnt = 0; //Scratching block count

/* FLASH verification status */
uint8_t VerifyStatus = 0;

/*********************************************************************
 * LOCAL FUNCTIONS
 */
void OTA_IAPReadDataComplete(unsigned char index);
void OTA_IAPWriteData(unsigned char index, unsigned char *p_data, unsigned char w_len);
void Rec_OTA_IAP_DataDeal(void);
void OTA_IAP_SendCMDDealSta(uint8_t deal_status);

static void Peripheral_ProcessTMOSMsg(tmos_event_hdr_t *pMsg);
static void peripheralStateNotificationCB(gapRole_States_t newState, gapRoleEvent_t *pEvent);
static void performPeriodicTask(void);
static void simpleProfileChangeCB(uint8_t paramID, uint8_t *pValue, uint16_t len);
static void peripheralParamUpdateCB(uint16_t connHandle, uint16_t connInterval,
        uint16_t connSlaveLatency, uint16_t connTimeout);
static void peripheralInitConnItem(peripheralConnItem_t *peripheralConnList);
static void peripheralRssiCB(uint16_t connHandle, int8_t rssi);
static void peripheralChar1Notify(uint8_t *pValue, uint16_t len);
/*********************************************************************
 * PROFILE CALLBACKS
 */
// GAP Role Callbacks
static gapRolesCBs_t Peripheral_PeripheralCBs = {
        peripheralStateNotificationCB, // Profile State Change Callbacks
        peripheralRssiCB,              // When a valid RSSI is read from controller (not used by application)
        peripheralParamUpdateCB
};

// Broadcast Callbacks
static gapRolesBroadcasterCBs_t Broadcaster_BroadcasterCBs = {
NULL, // Not used in peripheral role
        NULL  // Receive scan request callback
        };

// GAP Bond Manager Callbacks
static gapBondCBs_t Peripheral_BondMgrCBs = {
NULL, // Passcode callback (not used by application)
        NULL  // Pairing / Bonding state Callback (not used by application)
        };

// Simple GATT Profile Callbacks
static simpleProfileCBs_t Peripheral_SimpleProfileCBs = {
        simpleProfileChangeCB // Characteristic value change callback
        };

static OTAProfileCBs_t Peripheral_OTA_IAPProfileCBs = {
        OTA_IAPReadDataComplete, // Charactersitic value change callback
        OTA_IAPWriteData
};
/*********************************************************************
 * PUBLIC FUNCTIONS
 */

/*********************************************************************
 * @fn      Peripheral_Init
 *
 * @brief   Initialization function for the Peripheral App Task.
 *          This is called during initialization and should contain
 *          any application specific initialization (ie. hardware
 *          initialization/setup, table initialization, power up
 *          notificaiton ... ).
 *
 * @param   task_id - the ID assigned by TMOS.  This ID should be
 *                    used to send messages and set timers.
 *
 * @return  none
 */
void Peripheral_Init()
{
    Peripheral_TaskID = TMOS_ProcessEventRegister(Peripheral_ProcessEvent);
    uint8_t i = 0;
    uint8_t MacAddr[6];
    FLASH_GetMACAddress(MacAddr);
    // Setup the GAP Peripheral Role Profile
    {
        uint8_t initial_advertising_enable = TRUE;
        uint16_t desired_min_interval = DEFAULT_DESIRED_MIN_CONN_INTERVAL;
        uint16_t desired_max_interval = DEFAULT_DESIRED_MAX_CONN_INTERVAL;
//        sprintf(attDeviceName,"demo_%.2x%.2x%.2x",MacAddr[0],MacAddr[1],MacAddr[2]);
        memset(&scanRspData[11],0,20);
        scanRspData[9] = strlen(attDeviceName)+8;
        scanRspData[10] = GAP_ADTYPE_LOCAL_NAME_COMPLETE;
        sprintf(&scanRspData[11],"%s_%.2x%.2x%.2x",attDeviceName,MacAddr[0],MacAddr[1],MacAddr[2]);

        // Set the GAP Role Parameters
        GAPRole_SetParameter(GAPROLE_ADVERT_ENABLED,sizeof(uint8_t),&initial_advertising_enable);
        GAPRole_SetParameter(GAPROLE_SCAN_RSP_DATA,sizeof(scanRspData),scanRspData);

        for (i = 0;i<6;i++)
        {
            advertData[sizeof(advertData)-i-1] = MacAddr[i]; // Use chip mac address
        }

        GAPRole_SetParameter(GAPROLE_ADVERT_DATA,sizeof(advertData),advertData);
        GAPRole_SetParameter(GAPROLE_MIN_CONN_INTERVAL,sizeof(uint16_t),&desired_min_interval);
        GAPRole_SetParameter(GAPROLE_MAX_CONN_INTERVAL,sizeof(uint16_t),&desired_max_interval);
    }



    {
        uint16_t advInt = DEFAULT_ADVERTISING_INTERVAL;

        // Set advertising interval
        GAP_SetParamValue(TGAP_DISC_ADV_INT_MIN,advInt);
        GAP_SetParamValue(TGAP_DISC_ADV_INT_MAX,advInt);

        // Enable scan req notify
        GAP_SetParamValue(TGAP_ADV_SCAN_REQ_NOTIFY,ENABLE);
    }

    // Setup the GAP Bond Manager
    {
        uint32_t passkey = 0; // passkey "000000"
        uint8_t pairMode = GAPBOND_PAIRING_MODE_WAIT_FOR_REQ;
        uint8_t mitm = TRUE;
        uint8_t bonding = TRUE;
        uint8_t ioCap = GAPBOND_IO_CAP_DISPLAY_ONLY;
        GAPBondMgr_SetParameter(GAPBOND_PERI_DEFAULT_PASSCODE,sizeof(uint32_t),&passkey);
        GAPBondMgr_SetParameter(GAPBOND_PERI_PAIRING_MODE,sizeof(uint8_t),&pairMode);
        GAPBondMgr_SetParameter(GAPBOND_PERI_MITM_PROTECTION,sizeof(uint8_t),&mitm);
        GAPBondMgr_SetParameter(GAPBOND_PERI_IO_CAPABILITIES,sizeof(uint8_t),&ioCap);
        GAPBondMgr_SetParameter(GAPBOND_PERI_BONDING_ENABLED,sizeof(uint8_t),&bonding);
    }

    // Initialize GATT attributes
    GGS_AddService(GATT_ALL_SERVICES);           // GAP
    GATTServApp_AddService(GATT_ALL_SERVICES);   // GATT attributes
    DevInfo_AddService();                        // Device Information Service
    SimpleProfile_AddService(GATT_ALL_SERVICES); // Simple GATT Profile
    OTAProfile_AddService(GATT_ALL_SERVICES);

    // Set the GAP Characteristics
    GGS_SetParameter(GGS_DEVICE_NAME_ATT,strlen(attDeviceName)+7,&scanRspData[11]);
    // Setup the SimpleProfile Characteristic Values
    {
        uint8_t charValue1[SIMPLEPROFILE_CHAR1_LEN] = {0};
        SimpleProfile_SetParameter(SIMPLEPROFILE_CHAR1,SIMPLEPROFILE_CHAR1_LEN,charValue1);
    }

    // Init Connection Item
    peripheralInitConnItem(&peripheralConnList);

    // Register callback with SimpleGATTprofile
    SimpleProfile_RegisterAppCBs(&Peripheral_SimpleProfileCBs);

    // Register receive scan request callback
    GAPRole_BroadcasterSetCB(&Broadcaster_BroadcasterCBs);

    OTAProfile_RegisterAppCBs(&Peripheral_OTA_IAPProfileCBs);

    // Setup a delayed profile startup
    tmos_set_event(Peripheral_TaskID,SBP_START_DEVICE_EVT);
    tmos_start_task(Peripheral_TaskID,SBP_PERIODIC_EVT,1600);
}

/*********************************************************************
 * @fn      peripheralInitConnItem
 *
 * @brief   Init Connection Item
 *
 * @param   peripheralConnList -
 *
 * @return  NULL
 */
static void peripheralInitConnItem(peripheralConnItem_t *peripheralConnList)
{
    peripheralConnList->connHandle = GAP_CONNHANDLE_INIT;
    peripheralConnList->connInterval = 0;
    peripheralConnList->connSlaveLatency = 0;
    peripheralConnList->connTimeout = 0;
}

/*********************************************************************
 * @fn      Peripheral_ProcessEvent
 *
 * @brief   Peripheral Application Task event processor.  This function
 *          is called to process all events for the task.  Events
 *          include timers, messages and any other user defined events.
 *
 * @param   task_id  - The TMOS assigned task ID.
 * @param   events - events to process.  This is a bit map and can
 *                   contain more than one event.
 *
 * @return  events not processed
 */
uint16_t Peripheral_ProcessEvent(uint8_t task_id, uint16_t events)
{
    //  VOID task_id; // TMOS required parameter that isn't used in this function

    if(events&SYS_EVENT_MSG)
    {
        uint8_t *pMsg;

        if((pMsg = tmos_msg_receive(Peripheral_TaskID))!=NULL)
        {
            Peripheral_ProcessTMOSMsg((tmos_event_hdr_t *)pMsg);
            // Release the TMOS message
            tmos_msg_deallocate(pMsg);
        }
        // return unprocessed events
        return (events^SYS_EVENT_MSG);
    }

    if(events&SBP_START_DEVICE_EVT)
    {
        // Start the Device
        GAPRole_PeripheralStartDevice(Peripheral_TaskID,&Peripheral_BondMgrCBs,&Peripheral_PeripheralCBs);
        return (events^SBP_START_DEVICE_EVT);
    }

    if(events&SBP_PERIODIC_EVT)
    {
        // Restart timer
        tmos_start_task(Peripheral_TaskID,SBP_PERIODIC_EVT,2);
        performPeriodicTask();
        return (events^SBP_PERIODIC_EVT);
    }

    if(events&SBP_PARAM_UPDATE_EVT)
    {
        // Send connect param update request
        GAPRole_PeripheralConnParamUpdateReq(peripheralConnList.connHandle,
        DEFAULT_DESIRED_MIN_CONN_INTERVAL,
        DEFAULT_DESIRED_MAX_CONN_INTERVAL,
        DEFAULT_DESIRED_SLAVE_LATENCY,
        DEFAULT_DESIRED_CONN_TIMEOUT,
                Peripheral_TaskID);

        return (events^SBP_PARAM_UPDATE_EVT);
    }

    if(events&SBP_PHY_UPDATE_EVT)
    {
        // start phy update
        PRINT("PHY Update %x...\n",GAPRole_UpdatePHY(peripheralConnList.connHandle, 0, GAP_PHY_BIT_LE_2M,
                GAP_PHY_BIT_LE_2M, 0));

        return (events^SBP_PHY_UPDATE_EVT);
    }

    if(events&SBP_READ_RSSI_EVT)
    {
        GAPRole_ReadRssiCmd(peripheralConnList.connHandle);
        tmos_start_task(Peripheral_TaskID,SBP_READ_RSSI_EVT,SBP_READ_RSSI_EVT_PERIOD);
        return (events^SBP_READ_RSSI_EVT);
    }

    //OTA_FLASH_ERASE_EVT
    if(events&OTA_FLASH_ERASE_EVT)
    {
        uint8_t status;

        PRINT("ERASE:%08x num:%d\r\n",(int)(EraseAdd + EraseBlockCnt * FLASH_BLOCK_SIZE),(int )EraseBlockCnt);
        FLASH_Unlock();
        status = FLASH_ErasePage(EraseAdd+EraseBlockCnt*FLASH_BLOCK_SIZE);
        FLASH_Lock();

        /* Erase failed */
        if(status!=FLASH_COMPLETE)
        {
            OTA_IAP_SendCMDDealSta(status);
            return (events^OTA_FLASH_ERASE_EVT);
        }

        EraseBlockCnt++;

        /* End of erase */
        if(EraseBlockCnt>=EraseBlockNum)
        {
            PRINT("ERASE Complete\r\n");
            OTA_IAP_SendCMDDealSta(SUCCESS);
            return (events^OTA_FLASH_ERASE_EVT);
        }

        return (events);
    }

    // Discard unknown events
    return 0;
}

/*********************************************************************
 * @fn      Peripheral_ProcessGAPMsg
 *
 * @brief   Process an incoming task message.
 *
 * @param   pMsg - message to process
 *
 * @return  none
 */
static void Peripheral_ProcessGAPMsg(gapRoleEvent_t *pEvent)
{
    switch(pEvent->gap.opcode)
    {
    case GAP_SCAN_REQUEST_EVENT:
        {
//            PRINT("Receive scan req from %x %x %x %x %x %x  ..\n", pEvent->scanReqEvt.scannerAddr[0],
//                  pEvent->scanReqEvt.scannerAddr[1], pEvent->scanReqEvt.scannerAddr[2], pEvent->scanReqEvt.scannerAddr[3],
//                  pEvent->scanReqEvt.scannerAddr[4], pEvent->scanReqEvt.scannerAddr[5]);
        break;
    }

    case GAP_PHY_UPDATE_EVENT:
        {
        PRINT("Phy update Rx:%x Tx:%x ..\n",pEvent->linkPhyUpdate.connRxPHYS,pEvent->linkPhyUpdate.connTxPHYS);
        break;
    }

    default:
        break;
    }
}

/*********************************************************************
 * @fn      Peripheral_ProcessTMOSMsg
 *
 * @brief   Process an incoming task message.
 *
 * @param   pMsg - message to process
 *
 * @return  none
 */
static void Peripheral_ProcessTMOSMsg(tmos_event_hdr_t *pMsg)
{
    switch(pMsg->event)
    {
    case GAP_MSG_EVENT:
        {
        Peripheral_ProcessGAPMsg((gapRoleEvent_t *)pMsg);
        break;
    }

    case GATT_MSG_EVENT:
        {
        gattMsgEvent_t *pMsgEvent;

        pMsgEvent = (gattMsgEvent_t *)pMsg;
        if(pMsgEvent->method==ATT_MTU_UPDATED_EVENT)
        {
            peripheralMTU = pMsgEvent->msg.exchangeMTUReq.clientRxMTU;
            PRINT("mtu exchange: %d\n",pMsgEvent->msg.exchangeMTUReq.clientRxMTU);
        }
        break;
    }

    default:
        break;
    }
}

/*********************************************************************
 * @fn      Peripheral_LinkEstablished
 *
 * @brief   Process link established.
 *
 * @param   pEvent - event to process
 *
 * @return  none
 */
static void Peripheral_LinkEstablished(gapRoleEvent_t *pEvent)
{
    gapEstLinkReqEvent_t *event = (gapEstLinkReqEvent_t *)pEvent;

    // See if already connected
    if(peripheralConnList.connHandle!=GAP_CONNHANDLE_INIT)
    {
        GAPRole_TerminateLink(pEvent->linkCmpl.connectionHandle);
        PRINT("Connection max...\n");
    }
    else
    {
        peripheralConnList.connHandle = event->connectionHandle;
        peripheralConnList.connInterval = event->connInterval;
        peripheralConnList.connSlaveLatency = event->connLatency;
        peripheralConnList.connTimeout = event->connTimeout;
        peripheralMTU = ATT_MTU_SIZE;
        // Set timer for periodic event
        tmos_start_task(Peripheral_TaskID,SBP_PERIODIC_EVT,SBP_PERIODIC_EVT_PERIOD);

        // Set timer for param update event
        tmos_start_task(Peripheral_TaskID,SBP_PARAM_UPDATE_EVT,SBP_PARAM_UPDATE_DELAY);

        // Start read rssi
        //   tmos_start_task(Peripheral_TaskID, SBP_READ_RSSI_EVT, SBP_READ_RSSI_EVT_PERIOD);

        PRINT("Conn %x - Int %x \n",event->connectionHandle,event->connInterval);
    }
}

/*********************************************************************
 * @fn      Peripheral_LinkTerminated
 *
 * @brief   Process link terminated.
 *
 * @param   pEvent - event to process
 *
 * @return  none
 */
static void Peripheral_LinkTerminated(gapRoleEvent_t *pEvent)
{
    gapTerminateLinkEvent_t *event = (gapTerminateLinkEvent_t *)pEvent;

    if(event->connectionHandle==peripheralConnList.connHandle)
    {
        peripheralConnList.connHandle = GAP_CONNHANDLE_INIT;
        peripheralConnList.connInterval = 0;
        peripheralConnList.connSlaveLatency = 0;
        peripheralConnList.connTimeout = 0;
        tmos_stop_task(Peripheral_TaskID,SBP_PERIODIC_EVT);
        tmos_stop_task(Peripheral_TaskID,SBP_READ_RSSI_EVT);

        // Restart advertising
        {
            uint8_t advertising_enable = TRUE;
            GAPRole_SetParameter(GAPROLE_ADVERT_ENABLED,sizeof(uint8_t),&advertising_enable);
        }
    }
    else
    {
        PRINT("ERR..\n");
    }
}

/*********************************************************************
 * @fn      peripheralRssiCB
 *
 * @brief   RSSI callback.
 *
 * @param   connHandle - connection handle
 * @param   rssi - RSSI
 *
 * @return  none
 */
static void peripheralRssiCB(uint16_t connHandle, int8_t rssi)
{
//    PRINT("RSSI -%d dB Conn  %x \n", -rssi, connHandle);
}

/*********************************************************************
 * @fn      peripheralParamUpdateCB
 *
 * @brief   Parameter update complete callback
 *
 * @param   connHandle - connect handle
 *          connInterval - connect interval
 *          connSlaveLatency - connect slave latency
 *          connTimeout - connect timeout
 *
 * @return  none
 */
static void peripheralParamUpdateCB(uint16_t connHandle, uint16_t connInterval,
        uint16_t connSlaveLatency, uint16_t connTimeout)
{
    if(connHandle==peripheralConnList.connHandle)
    {
        peripheralConnList.connInterval = connInterval;
        peripheralConnList.connSlaveLatency = connSlaveLatency;
        peripheralConnList.connTimeout = connTimeout;

        PRINT("Update %x - Int %x \n",connHandle,connInterval);
    }
    else
    {
        PRINT("ERR..\n");
    }
}

/*********************************************************************
 * @fn      peripheralStateNotificationCB
 *
 * @brief   Notification from the profile of a state change.
 *
 * @param   newState - new state
 *
 * @return  none
 */
static void peripheralStateNotificationCB(gapRole_States_t newState, gapRoleEvent_t *pEvent)
{
    switch(newState&GAPROLE_STATE_ADV_MASK)
    {
    case GAPROLE_STARTED:
        PRINT("Initialized..\n");
        ble_state = DISCONNECTED;
        break;

    case GAPROLE_ADVERTISING:
        if(pEvent->gap.opcode==GAP_LINK_TERMINATED_EVENT)
        {
            Peripheral_LinkTerminated(pEvent);
            PRINT("Disconnected.. Reason:%x\n",pEvent->linkTerminate.reason);
            PRINT("Advertising..\n");
        }
        else if(pEvent->gap.opcode==GAP_MAKE_DISCOVERABLE_DONE_EVENT)
        {
            PRINT("Advertising..\n");
        }
        ble_state = DISCONNECTED;
        break;

    case GAPROLE_CONNECTED:
        if(pEvent->gap.opcode==GAP_LINK_ESTABLISHED_EVENT)
        {
            Peripheral_LinkEstablished(pEvent);
            PRINT("Connected..\n");
            ble_state = CONNECTED;
        }
        break;

    case GAPROLE_CONNECTED_ADV:
        if(pEvent->gap.opcode==GAP_MAKE_DISCOVERABLE_DONE_EVENT)
        {
            PRINT("Connected Advertising..\n");
            ble_state = CONNECTED;
        }
        break;

    case GAPROLE_WAITING:
        if(pEvent->gap.opcode==GAP_END_DISCOVERABLE_DONE_EVENT)
        {
            PRINT("Waiting for advertising..\n");
        }
        else if(pEvent->gap.opcode==GAP_LINK_TERMINATED_EVENT)
        {
            Peripheral_LinkTerminated(pEvent);
            PRINT("Disconnected.. Reason:%x\n",pEvent->linkTerminate.reason);
            ble_state = DISCONNECTED;
        }
        else if(pEvent->gap.opcode==GAP_LINK_ESTABLISHED_EVENT)
        {
            if(pEvent->gap.hdr.status!=SUCCESS)
            {
                PRINT("Waiting for advertising..\n");
            }
            else
            {
                PRINT("Error..\n");
            }
        }
        else
        {
            PRINT("Error..%x\n",pEvent->gap.opcode);
        }
        break;

    case GAPROLE_ERROR:
        PRINT("Error..\n");
        break;

    default:
        break;
    }
}

/*********************************************************************
 * @fn      performPeriodicTask
 *
 * @brief   Perform a periodic application task. This function gets
 *          called every five seconds as a result of the SBP_PERIODIC_EVT
 *          TMOS event. In this example, the value of the third
 *          characteristic in the SimpleGATTProfile service is retrieved
 *          from the profile, and then copied into the value of the
 *          the fourth characteristic.
 *
 * @param   none
 *
 * @return  none
 */
static void performPeriodicTask(void)
{
    struct AMessage tran_mes;
    if(xQueueReceive(ble_TxQueue,&tran_mes,0)==pdPASS)
    {
        peripheralChar1Notify(tran_mes.ucData,tran_mes.ucMessageLEN);
    }
}
/*********************************************************************
 * @fn      peripheralChar4Notify
 *
 * @brief   Prepare and send simpleProfileChar4 notification
 *
 * @param   pValue - data to notify
 *          len - length of data
 *
 * @return  none
 */
static void peripheralChar1Notify(uint8_t *pValue, uint16_t len)
{
    attHandleValueNoti_t noti;
    if(len>(peripheralMTU-3))
    {
        return;
    }
    noti.len = len;
    noti.pValue = GATT_bm_alloc(peripheralConnList.connHandle,ATT_HANDLE_VALUE_NOTI,noti.len,NULL,0);
    if(noti.pValue)
    {
        tmos_memcpy(noti.pValue,pValue,noti.len);
        if(simpleProfile_Notify(peripheralConnList.connHandle,&noti)!=SUCCESS)
        {

            GATT_bm_free((gattMsg_t *)&noti,ATT_HANDLE_VALUE_NOTI);
        }
    }
}

/*********************************************************************
 * @fn      simpleProfileChangeCB
 *
 * @brief   Callback from Profile indicating a value change
 *
 * @param   paramID - parameter ID of the value that was changed.
 *
 * @return  none
 */
static void simpleProfileChangeCB(uint8_t paramID, uint8_t *pValue, uint16_t len)
{
    switch(paramID)
    {
    case SIMPLEPROFILE_CHAR1:
        {
        struct AMessage txMSG;
        txMSG.ucMessageLEN = len;
        memcpy(txMSG.ucData,pValue,len);
        if(xQueueSend(ble_Queue,( void * )&txMSG,0)==pdPASS)
        {
//                printf("add to queue sucessfull\r\n");
        }
        break;
    }
    default:
        // should not reach here!
        break;
    }
}

/*********************************************************************
 * @fn      OTA_IAP_SendData
 *
 * @brief   OTA IAP sends data, which is limited to 20 bytes when used
 *
 * @param   p_send_data - Pointer to send data
 * @param   send_len    - Length of data sent
 *
 * @return  none
 */
void OTA_IAP_SendData(uint8_t *p_send_data, uint8_t send_len)
{
    OTAProfile_SendData(OTAPROFILE_CHAR,p_send_data,send_len);
}

/*********************************************************************
 * @fn      OTA_IAP_SendCMDDealSta
 *
 * @brief   Status return of OTA IAP execution
 *
 * @param   deal_status - Return state
 *
 * @return  none
 */
void OTA_IAP_SendCMDDealSta(uint8_t deal_status)
{
    uint8_t send_buf[2];

    send_buf[0] = deal_status;
    send_buf[1] = 0;
    OTA_IAP_SendData(send_buf,2);
}

/*********************************************************************
 * @fn      OTA_IAP_CMDErrDeal
 *
 * @brief   OTA IAP abnormal command code processing
 *
 * @return  none
 */
void OTA_IAP_CMDErrDeal(void)
{
    OTA_IAP_SendCMDDealSta(0xfe);
}

/*********************************************************************
 * @fn      SwitchImageFlag
 *
 * @brief   Switch ImageFlag in dataflash
 *
 * @param   new_flag    - Switching ImageFlag
 *
 * @return  none
 */
void SwitchImageFlag(uint8_t new_flag)
{
    /* Read the first block */
    FLASH_read(OTA_DATAFLASH_ADD,&block_buf[0],4);
    RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV2;
    __disable_irq();
    FLASH_Unlock_Fast();
    /* Erase the first block */
    FLASH_ErasePage_Fast( OTA_DATAFLASH_ADD);

    /* Update Image information */
    block_buf[0] = new_flag;
    block_buf[1] = 0x5A;
    block_buf[2] = 0x5A;
    block_buf[3] = 0x5A;

    /* Program DataFlash */
    FLASH_ProgramPage_Fast( OTA_DATAFLASH_ADD,(uint32_t *)&block_buf[0]);
    FLASH_Lock_Fast();
    RCC->CFGR0 &= ~(uint32_t)RCC_HPRE_DIV2;
    __enable_irq();
}

/*********************************************************************
 * @fn      DisableAllIRQ
 *
 * @brief   Disable all interrupts
 *
 * @return  none
 */
void DisableAllIRQ(void)
{
    __disable_irq();
}

/*********************************************************************
 * @fn      Rec_OTA_IAP_DataDeal
 *
 * @brief   Received OTA packet processing
 *
 * @return  none
 */
void Rec_OTA_IAP_DataDeal(void)
{
    IWDG_ReloadCounter();  //Feed dog
    switch(iap_rec_data.other.buf[0])
    {
    /* Programming */
    case CMD_IAP_PROM:
        {
//            uint32_t i;
//            FLASH_Status  status = FLASH_COMPLETE;

        OpParaDataLen = iap_rec_data.program.len;
        OpAdd = (uint32_t)(iap_rec_data.program.addr[0]);
        OpAdd |= ((uint32_t)(iap_rec_data.program.addr[1])<<8);
        OpAdd = OpAdd*16;

        OpAdd += IMAGE_A_SIZE+0x08000000;

        PRINT("IAP_PROM: %08x len:%d \r\n",(int )OpAdd,(int )OpParaDataLen);

        /* Current is ImageA, programming directly */
        tmos_memcpy(&block_buf[block_buf_len],iap_rec_data.program.buf,OpParaDataLen);
        block_buf_len += OpParaDataLen;
        if(block_buf_len>=FLASH_PAGE_SIZE)
        {
            RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV2;
            __disable_irq();
            FLASH_Unlock_Fast();
            FLASH_ProgramPage_Fast(prom_addr,(uint32_t*)block_buf);
            FLASH_Lock_Fast();
            RCC->CFGR0 &= ~(uint32_t)RCC_HPRE_DIV2;
            __enable_irq();
            tmos_memcpy(block_buf,&block_buf[FLASH_PAGE_SIZE],block_buf_len-FLASH_PAGE_SIZE);
            block_buf_len -= FLASH_PAGE_SIZE;
            prom_addr += FLASH_PAGE_SIZE;
        }
        OTA_IAP_SendCMDDealSta(SUCCESS);
        break;
    }
        /* Erase -- Bluetooth erase is controlled by the host */
    case CMD_IAP_ERASE:
        {
        OpAdd = (uint32_t)(iap_rec_data.erase.addr[0]);
        OpAdd |= ((uint32_t)(iap_rec_data.erase.addr[1])<<8);
        OpAdd = OpAdd*16;

        OpAdd += IMAGE_A_SIZE+0x08000000;

        EraseBlockNum = (uint32_t)(iap_rec_data.erase.block_num[0]);
        EraseBlockNum |= ((uint32_t)(iap_rec_data.erase.block_num[1])<<8);
        EraseAdd = OpAdd;
        EraseBlockCnt = 0;

        /* The inspection is placed in the era of clearing 0 */
        VerifyStatus = 0;

        prom_addr = IMAGE_B_START_ADD;
        PRINT("IAP_ERASE start:%08x num:%d\r\n",(int )EraseAdd,(int )EraseBlockNum);

        if((EraseAdd<IMAGE_B_START_ADD)||((EraseAdd+(EraseBlockNum-1)*FLASH_BLOCK_SIZE)>(IMAGE_B_START_ADD+IMAGE_B_SIZE)))
        {
            OTA_IAP_SendCMDDealSta(0xFF);
        }
        else
        {
            /* Start erasing */
            tmos_set_event(Peripheral_TaskID,OTA_FLASH_ERASE_EVT);
        }
        break;
    }
        /* Verify */
    case CMD_IAP_VERIFY:
        {
//            uint32_t i;
        uint8_t status = 0;
        uint8_t verifyData[iap_rec_data.verify.len];

        if(block_buf_len)
        {
            RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV2;
            __disable_irq();
            FLASH_Unlock_Fast();
            FLASH_ProgramPage_Fast(prom_addr,(uint32_t*)block_buf);
            FLASH_Lock_Fast();
            RCC->CFGR0 &= ~(uint32_t)RCC_HPRE_DIV2;
            __enable_irq();
            block_buf_len = 0;
            prom_addr = 0;
        }

        OpParaDataLen = iap_rec_data.verify.len;

        OpAdd = (uint32_t)(iap_rec_data.verify.addr[0]);
        OpAdd |= ((uint32_t)(iap_rec_data.verify.addr[1])<<8);
        OpAdd = OpAdd*16;

        OpAdd += IMAGE_A_SIZE+0x08000000;
        PRINT("IAP_VERIFY: %08x len:%d \r\n",(int )OpAdd,(int )OpParaDataLen);
        FLASH_read(OpAdd,verifyData,OpParaDataLen);
        /* Currently ImageA, read ImageB for verification directly*/
        status = tmos_memcmp(verifyData,iap_rec_data.verify.buf,OpParaDataLen);
        if(status==FALSE)
        {
            PRINT("IAP_VERIFY err \r\n");
            VerifyStatus = 0xFF;
        }
        OTA_IAP_SendCMDDealSta(VerifyStatus);
        break;
    }
        /* End of rogramming */
    case CMD_IAP_END:
        {
        PRINT("IAP_END \r\n");

        /* The current one is ImageA */
        /* Close all the current use interrupt, or it is convenient to directly close */
        DisableAllIRQ();

        /* Modify DataFlash and switch to ImageIAP */
        SwitchImageFlag(IMAGE_IAP_FLAG);

        /* Wait for printing to complete, reset*/
        Delay_Ms(10);
        NVIC_SystemReset();

        break;
    }
    case CMD_IAP_INFO:
        {
        uint8_t send_buf[20];

        PRINT("IAP_INFO \r\n");
        /* IMAGE FLAG */
        send_buf[0] = IMAGE_B_FLAG;

        /* IMAGE_SIZE */
        send_buf[1] = (uint8_t)(IMAGE_SIZE&0xff);
        send_buf[2] = (uint8_t)((IMAGE_SIZE>>8)&0xff);
        send_buf[3] = (uint8_t)((IMAGE_SIZE>>16)&0xff);
        send_buf[4] = (uint8_t)((IMAGE_SIZE>>24)&0xff);

        /* BLOCK SIZE */
        send_buf[5] = (uint8_t)(FLASH_BLOCK_SIZE&0xff);
        send_buf[6] = (uint8_t)((FLASH_BLOCK_SIZE>>8)&0xff);

        send_buf[7] = CHIP_ID&0xFF;
        send_buf[8] = (CHIP_ID>>8)&0xFF;
        /* Add more if necessary */

        /* send message */
        OTA_IAP_SendData(send_buf,20);

        break;
    }

    default:
        {
        OTA_IAP_CMDErrDeal();
        break;
    }
    }
}

/*********************************************************************
 * @fn      OTA_IAPReadDataComplete
 *
 * @brief   OTA data reading complete processing
 *
 * @param   index   - OTA channel serial number
 *
 * @return  none
 */
void OTA_IAPReadDataComplete(unsigned char index)
{
    PRINT("OTA Send Comp \r\n");
}

/*********************************************************************
 * @fn      OTA_IAPWriteData
 *
 * @brief   OTA channel data receiving complete processing
 *
 * @param   index   - OTA channel serial number
 * @param   p_data  - Written data
 * @param   w_len   - Length
 *
 * @return  none
 */
void OTA_IAPWriteData(unsigned char index, unsigned char *p_data, unsigned char w_len)
{
    unsigned char rec_len;
    unsigned char *rec_data;

    rec_len = w_len;
    rec_data = p_data;
    tmos_memcpy((unsigned char *)&iap_rec_data,rec_data,rec_len);
    Rec_OTA_IAP_DataDeal();
}

/*********************************************************************
 * @fn      FLASH_read
 *
 * @brief   Read flash
 *
 * @return  none
 */
void FLASH_read(uint32_t addr, uint8_t *pData, uint32_t len)
{
    uint32_t i;
    for (i = 0;i<len;i++)
    {
        *pData++ = *(uint8_t*)addr++;
    }
}

/*********************************************************************
 *********************************************************************/
